机械臂碰撞功能开发流程

[TOC]

机器人运行在位置环时，才能使用碰撞相关算法。

1 设置相关参数

1.1 调用如下接口，初始化算法库中机器人状态；

    /**
     * @brief 初始化算法库内部机械臂的状态
     *  切换控制器之前, 需要先调用这个函数
     */
    ARAL_API_COMMON(1.0) int rsInitiateRobotState() = 0;

1.2 调用如下接口，设置重力方向；

    /**
     * @brief 设置重力加速度向量在机械臂基座坐标系下的描述
     * @param vec[0]: 重力加速度在机械臂基座坐标系X轴上的分量
     *        vec[1]: 重力加速度在机械臂基座坐标系Y轴上的分量
     *        vec[2]: 重力加速度在机械臂基座坐标系Z轴上的分量
     * @return if < 0, 表示设置失败
     */
    ARAL_API_COMMON(1.0) int mdlSetGravityVectorInBase(const Array3d& vec) = 0;

1.3 调用如下接口，设置工具的动力学参数；

    /**
      * @brief 设置机械臂的连杆动力学参数
      * @param real_para: 实际辨识出来的参数，参数格式为：real_para = [参数类型, 辨识的参数]
      * 1. real_para[0] = 0; size(real_para) = 60 + 1, 输入参数含义为 m, mx, my, mz, ixx, ixy, ixz, iyy, iyz, izz ..., 需要另外设置转子惯量．否则为urdf里面的值．
      * 2. real_para[0] = 1; size(real_para) = 58 + 1, 对应　aral　动力学标定接口的数据输出格式.
      * 3. real_para[0] = 2; size(real_para) = 42 + 1, 对应第一版拖动示教导入生产写在底座的数据格式．
      * @return: if < 0, 则设置的参数类型或大小不正确
      */
    ARAL_API_COMMON(1.0) int mdlSetRobotLinkDynamicParameter(const std::vector<double>& real_para) = 0;

1.4 调用如下接口，设置机器人的动力学参数；

/**
 * @brief 设置工具的动力学参数, 参考坐标系为末端传感器坐标系
 * @param inertial: 工具的动力学参数结构体
 */
 ARAL_API_COMMON(1.0) void mdlSetToolDynamicParameter(const RLInertia& inertia) = 0;
// 读取用户通过示教器输入的参数

1.5调用如下接口，设置关节摩擦参数；

    /**
     * @brief 设置关节摩擦参数, 该参数一般通过读取关节驱动中的参数而获得
     * @param friPara: 摩擦力参数结构体，具体的摩擦参数模型咨询算法部
     * @return < 0, 表示设置失败
     */
    ARAL_API_COMMON(1.0) int mdlSetJointFrictionParameter(const std::vector<FrictionParam>& friPara) = 0;

1.6 调用如下硬件抽象层的滤波设置接口，分别设置关节电流、速度、加速度滤波参数(非canlite协议无加速度设置)

    int setCurrentFilterFrequency(const std::vector<int> &cur_hz);
    int setVelocityFilterFrequency(const std::vector<int> &vel_hz);
    int setAccelerationFilterFrequency(const std::vector<int> &vel_hz);

注意：第1节中所有需要设置的参数典型值，请参考minico工程aral_collisiondetect_validate.cpp。

2 开启碰撞检测&处理功能

2.1 更新算法库state模块状态

开启碰撞检测功能之前，需要更新算法库state状态，一共有五步，分别为更新关节运动状态（rsUpdateJointPVA)、更新关节动力学状态(rsUpdateJointCTF)、设置目标力(rsSetGoalForce)、设置参考轨迹(rsSetReferenceTrajectory)、计算控制输出(rsCalJointCommand)。所有需要设置的参数典型值，请参考minico工程aral_collisiondetect_validate.cpp。

    /**
     * @brief 更新机器人关节的实时状态信息。(如果用户不能给出全部信息, 可以先调用 utlKalmanFilterPVA 函数进行估计)
     *        调用这个接口会连带更新机械臂所有连杆在笛卡尔空间的运动学信息, 包括位姿, 速度和加速度等
     * @param q: 关节当前位置
     * @param qd: 关节当前速度
     * @param qdd: 关节当前加速度
     * @param　is_user_acc: 是否采用用户输入的加速度(默认是否)
     * @return if < 0, 表示设置失败
     */
    ARAL_API_COMMON(1.0) int rsUpdateJointPVA(const RLJntArray& q, const RLJntArray& qd, const RLJntArray& qdd, const bool& is_user_acc = false) = 0;

    /**
     * @brief 更新机器人关节的实时关节力矩/摩擦和温度信息, 用于碰撞监测或估计外力
     * @param toruqe: 关节实际力矩, 单位Nm
     * @param temperature: 关节温度, 单位为摄氏度(℃)
     * @param fricition: 关节摩擦力，单位为Nm
     * @return if < 0, 表示设置失败
     */
    ARAL_API_COMMON(1.0) int rsUpdateJointCTF(const RLJntArray& toruqe, const RLJntArray& temperature, const RLJntArray& friction) = 0;

    /**
     * @brief 设置机械臂在控制坐标系下的目标力/力矩(如果是动态坐标系或者时变力,则每个控制周期都需要设置)
     * @param space: 控制坐标系类型
     * @param pose: 如果space不是关节空间,则表示控制坐标系相对基坐标系的相位位姿
     * @param force: 目标力/力矩(一般在笛卡尔空间描述, 在关节空间不存在目标输出力矩)
     * @return: if < 0, 表示计算失败
     */
    ARAL_API_COMMON(1.0) int rsSetGoalForce(const CoordType& space, const RLPose& frame, const RLWrench& force) = 0;

    /**
     * @brief 设置参考轨迹(用户设置参考位置时，必须同时设置参考速度和参考加速度;否则可以都设置为空)
     * @param type: 参考轨迹描述坐标系,具体可参考CoordType定义
     * @param frame: 如果type不等于JOINT, 则frame表示参考轨迹坐标系相对基坐标系的相对位姿关系
     * @param positions: 参考轨迹的位置信息, 如没有, 则置空
     * @param velocities: 参考轨迹的速度信息, 如没有, 则置空
     * @param accelerations: 参考轨迹的加速度信息, 如没有, 则置空
     * @return if < 0, 则表示设置失败
     */
    ARAL_API_COMMON(1.0) int rsSetReferenceTrajectory(const CoordType& type, const RLPose& frame, const double* positions,const double* velocities, const double* accelerations) = 0;

        /**
     * @brief 计算关节的控制指令参数.(调用该函数时, 如果算法出错, 则返回数据不可用)
     * @param res: 返回值, 如果是位置控制, 则表示关节角度; 如果是力矩控制,  则表示关节电流
     * @param qd:  返回值, 如果是位置控制, 则表示关节指令速度; 否则无意义
     * @param qdd: 返回值, 如有是位置控制, 则表示关节指令加速度; 否则无意义
     * @param ik_eps: 逆解迭代精度参数
     * @return return: 0 - 计算正确
     *            -60003 - 运动速度超限，解决方法：调整力控参数
     *             65001 - 机器人没有参考轨迹，解决方法：调用rsSetReferenceTrajectory设置参考轨迹
     *            -30027 - 没有和参考角在同一关节子空间的逆解，解决方法：未实现在子空间内搜索逆解, 建议更改选解条件，RobotConfiguration::NONE
     *            -30006 - IK 计算失败；解决方法：机械臂在当前位置没有解，且没有使能迭代解,　建议使能迭代解
     *            -40055 - 速度规划失败，解决方法：检查始末速度是否超过运动约束、检查运动约束是否大于0
     */
    ARAL_API_COMMON(1.0) int rsCalJointCommand(RLJntArray& res, RLJntArray& qd, RLJntArray& qdd, const Array2d& ik_eps = {ARAL_IK_ITER_POS_EPS, ARAL_IK_ITER_ORI_EPS}) = 0;

2.2 开启碰撞检测

判断是否发生碰撞，调用如下接口获得碰撞状态；

    /**
     * @brief 检测机器人是否发生碰撞(周期性调用，update机械臂的运动学参数和动力学参数后即可调用这个函数)
     * @param collision_threshold: 碰撞检测阈值，单位Nm
     * @param result: 碰撞结果
     * @return: if < 0, 表示计算失败
     */
    ARAL_API_COMMON(1.0) int rsDetectJointCollision(const RLJntArray& collision_threshold, JointCollisionResult& result)const = 0;

其中collision_threshold为检测阈值，根据算法部提供的阈值表及用户输入的碰撞等级进行设置；result为碰撞检测结果，其具体结构如下

struct JointCollisionResult
{
    bool                collision{false};                       // 是否发生碰撞
    std::vector<bool>   collision_flag;                         // 碰撞关节标志
    RLJntArray          contact_torque;                         // 碰撞力矩
};//碰撞数据

一般情况下，直接采用布尔数值result.collision作为最终判断结果即可。

2.3 碰撞反应

发生碰撞后(result.collision结果为true)，直接向驱动层下发碰撞指令即可。